#include "BugSimple.h"
#include "imageloader.h"

static int x1 = 0, x2 = 0, x3 = 0, x4= 0, x5 = 0;
int i, j;
int num= 6; // number of legs
int dub = 1; // which camera to load texture to
int renew= 0;
double len = 10;
float body[4][3] = {{2,1,5},{2,1,6},{2,2,4}, {4,4,8}};
float bodyOffset[3][3] = {{0, 1, 0},{0, 1, -6},{0, 1, 6}};
float bodyOffset1[2][3] = {{0, 1, -4},{0, 1, 8}};
float _angleT2=0;
GLuint _textureId;

valarray<float> LegAngles( 30 );
valarray<float> BodyT(3);
valarray<float> BodyR(3);	

// a simple function to generate some parameteters
void GenerateParams1(float t) {
	float a=-45;
	float b=90;
	float c=-90;	
	for( int i = 0; i < num; i++ ) {
		LegAngles[i*5 + 0] = a + 20*sin( (float(i)+1.0)*t );
		LegAngles[i*5 + 1] = b + 20*sin( 1.2*(float(i)+1.0)*t );
		LegAngles[i*5 + 2] = c + 20*sin( 0.9*(float(i)+1.0)*t );
		LegAngles[i*5 + 3] = 0;
		LegAngles[i*5 + 4] =0;
	}

	BodyT[0] = 20*sin(t);
	BodyT[1] = 0;
	BodyT[2] = 20*cos(t);

}


void GenerateParams2Walking(float t) {

	float a = -45;
	float b = 90;
	float c = -90;	
	float _angleT;


	_angleT=10*sin(t/.1);
	_angleT2 = 0*-5*cos(t/.05);
	/*_angleT=10*sin(t/.05);
	_angleT2 = -5*cos(t/.05);*/


	for( int i = 0; i < num; i++ ) { // num == 6
		LegAngles[i*5 + 0] = a ;
		LegAngles[i*5 + 1] = b ;
		LegAngles[i*5 + 2] = c ;
		LegAngles[i*5 + 3] =(_angleT);
		LegAngles[i*5 + 4] =(_angleT2); 
	} // total of 6*5 = 30 params (5 per leg, 6 legs)
	
	
	float d = 18.437;
	bool bUsety = false;
	float ty = 10 * !bUsety + bUsety*-22.36*sin((3.14159265/180)*(-d-abs(_angleT2)));

	BodyT[0] = 0;
	BodyT[1] = ty;
	BodyT[2] = t/.05-renew*200;

	BodyR[0]=2.5*sin(t/.05);
	 if (BodyT[0] > 100)
	 {
		 renew++;
	 }
	//_angle = sin(0.1*t);

}

void PassParams(const valarray<float>& inAng, const valarray<float>& inBodyT,
			   const valarray<float>& inBodyR ) {	
	LegAngles = inAng;
	BodyR = inBodyR;
	BodyT = inBodyT;
}



// get the current param state (e.g. to check vs. tracked value)
void GetParams(valarray<float>& outAng, valarray<float>& outBodyT,
			   valarray<float>& outBodyR ) {
	outAng = LegAngles;
	outBodyT = BodyT;
	outBodyR = BodyR;
}

void SetBodyRT( float tx, float ty, float tz,
			    float wx, float wy, float wz )
{
	// convention: first rotate, then translate
	BodyT[0] = tx; BodyT[1] = ty; BodyT[2] = tz; 
	
	// to do: getting the body R correctly here
	float mag = sqrt(BodyR[0]*BodyR[0]+BodyR[1]*BodyR[1]+BodyR[2]*BodyR[2]);

}

void light()
{
	glEnable(GL_LIGHTING);
	/*glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);
	glEnable(GL_LIGHT2);
	glEnable(GL_LIGHT3);*/

	//Ambient Light
	float ambientColor[]= {1.0,1.0,1.0, 1 };
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientColor);
	
	//Positioned Light 1
	float lightColor0[] = {.5f, .95f, 0.5f};
	float lightPos0[] = {10.0f, 15.0f, 10.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightColor0);
	glLightfv(GL_LIGHT0, GL_POSITION, lightPos0);

	//Directed Light 2
	float lightColor1[] = {.5f, .95f, .5f};
	float lightPos1[] = {-10.0f, 15.0f, -10.0f, 1.0f};
	glLightfv(GL_LIGHT1, GL_DIFFUSE, lightColor1);
	glLightfv(GL_LIGHT1, GL_POSITION, lightPos1);

	//Directed Light 2
	float lightColor2[] = {.5f, .95f, .95f};
	float lightPos2[] = {-10.0f, 5.0f, 10.0f, 1.0f};
	glLightfv(GL_LIGHT2, GL_DIFFUSE, lightColor2);
	glLightfv(GL_LIGHT2, GL_POSITION, lightPos2);

	//Directed Light 2
	float lightColor3[] = {.95f, .5f, .95f};
	float lightPos3[] = {10.0f, 5.0f, -10.0f, 1.0f};
	glLightfv(GL_LIGHT3, GL_DIFFUSE, lightColor3);
	glLightfv(GL_LIGHT3, GL_POSITION, lightPos3);
	

}

void drawLimb(float scaleF, float trans)
{
	//limb	
	glPushMatrix();
	glTranslatef(scaleF/2, 0.0f, 0.0f);	
	glScalef(scaleF, 1 , 1 );
	//glColor3f(.5,.5,.5);
	glutSolidSphere(.5,100,100);
	glPopMatrix();
}
void drawJoint(int scale)
{	
	glPushMatrix();
	//glColor3f(.5,.5,.5);
	glScalef(scale,scale,scale);
	glutSolidSphere(.5,100,100);
	glPopMatrix();
}
void arm(float a, float b, float c, float d, float e,  double x, double y, double z, int i, int gh)
{
	glPushMatrix();
	glTranslatef(x,y,z);
	glRotatef(-(180/(3.14159265))*atan2(z , x)-(d*pow((-1),double (i%2))), 0.0f, 1.0f , 0.0f);
	glRotatef(-e*pow((-1),double (gh%2)), 0.0f, 0.0f , 1.0f);
	glPushMatrix();

	//first Limb	
	glRotatef(a, 0.0f, 0.0f , 1.0f);	
	drawLimb(5, 0);
	drawJoint(1);

	//second Limb	
	glTranslatef(5,0,0);
	glRotatef(b, 0.0f, 0.0f , 1.0f);	
	drawLimb(10, 0);
	drawJoint(1);

	//third Limb
	glTranslatef(10,0,0);
	glRotatef(c, 0, 0, 1);
	drawLimb(15, 0);
	drawJoint(1);
	glPopMatrix();
	glPopMatrix();
}

void legLocation (double N, double M) {
	
	double inc;
	double *loc = new double[num*3];

	inc=len/(num/2+1);
	for (i=0 ; i < num/2 ; i++ )
	{	
		loc[3*i]=sqrt(N*N*(1-(len/2-(inc*(i+1)))*(len/2-(inc*(i+1)))/M/M));
		loc[3*i+1]=0;
		loc[3*i+2]=-(len/2-(inc*(i+1)));

	}	
	
	for( i=0 ; i < num/2; i++ )
		{	
		float a = LegAngles[i*5 + 0];
		float b = LegAngles[i*5 + 1];
		float c = LegAngles[i*5 + 2];
		float d = LegAngles[i*5 + 3];
		float e = LegAngles[i*5 + 4];
			arm(a,b,c,d,e,loc[3*i]*pow((-1),double (i%2)),loc[3*i+1],loc[3*i+2],i,1);
		}
	for( i=num/2 ; i < num; i++ )
		{	
		float a = LegAngles[i*5 + 0];
		float b = LegAngles[i*5 + 1];
		float c = LegAngles[i*5 + 2];
		float d = LegAngles[i*5 + 3];
		float e = LegAngles[i*5 + 4];
			arm(a,b,c,d,e,-loc[3*(i-num/2)]*pow((-1),double ((i-num/2)%2)),-loc[3*(i-num/2)+1],loc[3*(i-num/2)+2],i+1,0);
		}
	delete loc;
}
void drawBody (void)
{
	//MidBody
	glPushMatrix();
	glScalef(body[0][0],body[0][1],body[0][2]);
	//glColor3f(.5,.5,.5);
	glutSolidSphere(1,100,100);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(bodyOffset[0][0],bodyOffset[0][1],bodyOffset[0][2]);	
	glScalef(body[1][0],body[1][1]+1,body[1][2]);
	//glColor3f(.5,.5,.5);
	glutSolidSphere(1,100,100);
	glPopMatrix();


	//head
	glPushMatrix();
	glTranslatef(bodyOffset[1][0],bodyOffset[1][1],bodyOffset[1][2]);	
	drawJoint(1);
	glRotatef(-15, 1.0f, 0.0f , 0.0f);
	glTranslatef(bodyOffset1[0][0],bodyOffset1[0][1],bodyOffset1[0][2]);
	glScalef(body[2][0],body[2][1],body[2][2]);
	//glColor3f(.5,.5,.5);
	glutSolidSphere(1,100,100);
	glPopMatrix();

	//thorax
	glPushMatrix();	
	glTranslatef(bodyOffset[2][0],bodyOffset[2][1],bodyOffset[2][2]);
	drawJoint(1);
	glRotatef(-15, 1.0f, 0.0f , 0.0f);
	glTranslatef(bodyOffset1[1][0],bodyOffset1[1][1],bodyOffset1[1][2]);
	glScalef(body[3][0],body[3][1],body[3][2]);
	//glColor3f(.5,.5,.5);
	glutSolidSphere(1,100,100);
	glPopMatrix();
}



//Makes the image into a texture, and returns the id of the texture
GLuint loadTexture(Image* image) {
	GLuint textureId;
	glGenTextures(1, &textureId); //Make room for our texture
	glBindTexture(GL_TEXTURE_2D, textureId); //Tell OpenGL which texture to edit
	//Map the image to the texture
	glTexImage2D(GL_TEXTURE_2D,                //Always GL_TEXTURE_2D
				 0,                            //0 for now
				 GL_RGB,                       //Format OpenGL uses for image
				 image->width, image->height,  //Width and height
				 0,                            //The border of the image
				 GL_RGB, //GL_RGB, because pixels are stored in RGB format
				 GL_UNSIGNED_BYTE, //GL_UNSIGNED_BYTE, because pixels are stored
				                   //as unsigned numbers
				 image->pixels);               //The actual pixel data
	return textureId; //Returns the id of the texture
}
void texturesetup()
{
	Image* image = loadBMP("vtr.bmp");
	_textureId = loadTexture(image);
	delete image;
}

void background()
{
	
	if( dub <= NUMCAMS ) {
		Image* image = loadBMP("test.bmp");
		_textureId = loadTexture(image);
		delete image;
		dub++;
	}
	
	

	glPushMatrix();
	//glScalef(5,5,10);
	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, _textureId);
	
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glBegin(GL_QUADS);
		glNormal3f(0.0, 0.0f, -1.0f);
		glTexCoord2f(0.0f, 0.0f);
		glVertex3f(-100.0f, -100.0f, 0);
		glTexCoord2f(1.0f, 0.0f);
		glVertex3f(-100.0f, 100.0f, 0);
		glTexCoord2f(1.0f, 1.0f);
		glVertex3f(100.0f, 100.0f,0);
		glTexCoord2f(0.0f,1.0f);
		glVertex3f(100.0f, -100.0f, 0);
	glEnd();

	glPopMatrix();
	glDisable(GL_TEXTURE_2D);

}

void drawBugModel()
{
	glDisable(GL_LIGHTING);
	
	//necessary to begin the production of the structures
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	//floor axis
	glPushMatrix();
	glScalef(10.0,10.0,10.0);
	glPopMatrix();
	//Bug and Body frame axis
	glPushMatrix();

	//glTranslatef(0,-22.36*sin((3.14159265/180)*(-d-abs(_angleT2))),0);
	
	glRotatef(BodyR[0], 0.0f, 1.0f , 0.0f); // BODY ROTATION <CAREFUL!  this is rotation
	// is by *amount* bodyR[0] about [0,1,0], not the full bodyR rotation vector as expected!
	// suggested use: rotate about each axis by one of the BodyR components (?)
		
	{ // BODY TRANSLATION OF AXES
		glPushMatrix();
		glTranslatef(BodyT[0],BodyT[1], BodyT[2] ); 
		glScalef(5.0,5.0,5.0);
		glPopMatrix();
	}
	{   // BODY TRANSLATION OF BUG
		glTranslatef( BodyT[0],BodyT[1], BodyT[2] ); 
		glRotatef(180, 0.0f, 1.0f , 0.0f);	
		glColor3f(1.0,1.0,1.0);
		drawBody();
		legLocation(body[0][0],body[0][2]);
		glPopMatrix();
	}

	//The Floor
	glTranslatef(0.0f ,0.0f ,0.0f);
	glRotatef(90,0,1,0);
	glRotatef(90,1,0,0);
	
}

void drawBug()
{
	//necessary to begin the production of the structures
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	bool bDrawAxes = false;

	/*float d;
	d=18.437;*/
	light();
	
	//floor axis
	glPushMatrix();
	glScalef(10.0,10.0,10.0);
	if( bDrawAxes )
		DrawAxes();
	glPopMatrix();
	//Bug and Body frame axis
	glPushMatrix();
	
	//glTranslatef(0,-22.36*sin((3.14159265/180)*(-d-abs(_angleT2))),0);
	
	
	glRotatef(BodyR[0], 0.0f, 1.0f , 0.0f); // BODY ROTATION <CAREFUL!  this is rotation
	// is by *amount* bodyR[0] about [0,1,0], not the full bodyR rotation vector as expected!
	// suggested use: rotate about each axis by one of the BodyR components (?)
	glPushMatrix();
	glTranslatef(BodyT[0],BodyT[1], BodyT[2] ); // BODY TRANSLATION OF AXES
	glScalef(5.0,5.0,5.0);
	if( bDrawAxes )
		DrawAxes();
	glPopMatrix();
	glTranslatef( BodyT[0],BodyT[1], BodyT[2] ); // BODY TRANSLATION OF BUG
	glRotatef(180, 0.0f, 1.0f , 0.0f);	
	glColor3f(0.5,0.5,0.5);
	drawBody();
	legLocation(body[0][0],body[0][2]);
	glPopMatrix();

	//The Floor
	glTranslatef(0.0f ,0.0f ,0.0f);
	glRotatef(90,0,1,0);
	glRotatef(90,1,0,0);
	glPushMatrix();
	glColor3f(0.0,0.0,0.0);

	background();
	glPopMatrix();

	
}
